BCD_To_CHAR:
	

	mov r17, r16
	swap r17

	andi r17, 0x0f
	ori r17, 0x30

	andi r16, 0x0f
	ori r16, 0x30

	st Y+, r17
	st Y, r16

ret

Bin_To_ASCII:
	push r16
	push r17
	push r18
	push r20
	push r21
	push r22
	push r23

	LD r18, X+
	LD r17, X+
	LD r16, X

	rcall Bin3BCD16

	mov r16, r23
	swap r16
	andi r23, 0x0f
	ori r23, 0x30
	andi r16, 0x0f
	ori r16, 0x30
	st Y+, r16
	st Y+, r23
	
	mov r16, r22
	swap r16
	andi r22, 0x0f
	ori r22, 0x30
	andi r16, 0x0f
	ori r16, 0x30
	st Y+, r16
	st Y+, r22
	
	mov r16, r21
	swap r16
	andi r21, 0x0f
	ori r21, 0x30
	andi r16, 0x0f
	ori r16, 0x30
	st Y+, r16
	st Y+, r21
	
	mov r16, r20
	swap r16
	andi r20, 0x0f
	ori r20, 0x30
	andi r16, 0x0f
	ori r16, 0x30
	st Y+, r16	
	st Y+, r20

	ldi r16, 0
	st Y, r16 // end of string

	pop r23
	pop r22
	pop r21
	pop r20
	pop r18
	pop r17
	pop r16
ret
                             
;***************************************************************************
;*
;* Bin3BCD == 24-bit Binary to BCD conversion
;*
;* fbin0:fbin1:fbin2  >>>  tBCD0:tBCD1:tBCD2:tBCD3
;*	  hex			     dec
;*     r16r17r18      >>>	r20r21r22r23
;*
;***************************************************************************
.def	fbin0	=r16	; binary value byte 0 (LSB)
.def	fbin1	=r17	; binary value byte 1
.def	fbin2	=r18	; binary value byte 2 (MSB)

.def	tBCD0	=r20	; BCD value digits 0 and 1
.def	tBCD1	=r21	; BCD value digits 2 and 3
.def	tBCD2	=r22	; BCD value digits 4 and 5
.def	tBCD3	=r23	; BCD value digits 6 and 7 (MSD)
                                    
Bin3BCD16:	
	ldi	tBCD3,0xfa		;initialize digits 7 and 6
binbcd_107:	
	subi	tBCD3,-0x10		;
    subi	fbin0,byte1(10000*1000) ;subit fbin,10^7
    sbci	fbin1,byte2(10000*1000) ;
    sbci	fbin2,byte3(10000*1000) ;
    brcc	binbcd_107		;
binbcd_106:	
	dec	tBCD3			;
    subi	fbin0,byte1(-10000*100) ;addit fbin,10^6
    sbci	fbin1,byte2(-10000*100) ;
    sbci	fbin2,byte3(-10000*100) ;
    brcs	binbcd_106		;
    ldi	tBCD2,0xfa		;initialize digits 5 and 4
binbcd_105:	
	subi	tBCD2,-0x10		;
    subi	fbin0,byte1(10000*10)	;subit fbin,10^5
    sbci	fbin1,byte2(10000*10)	;
    sbci	fbin2,byte3(10000*10)	;
    brcc	binbcd_105		;
binbcd_104:	
	dec	tBCD2			;
    subi	fbin0,byte1(-10000)	;addit fbin,10^4
    sbci	fbin1,byte2(-10000)	;
    sbci	fbin2,byte3(-10000)	;
    brcs	binbcd_104		;
    ldi	tBCD1,0xfa		;initialize digits 3 and 2
binbcd_103:	
	subi	tBCD1,-0x10		;
    subi	fbin0,byte1(1000)	;subiw fbin,10^3
    sbci	fbin1,byte2(1000)	;
    brcc	binbcd_103		;
binbcd_102:	
	dec	tBCD1			;
    subi	fbin0,byte1(-100)	;addiw fbin,10^2
    sbci	fbin1,byte2(-100)	;
    brcs	binbcd_102		;
    ldi	tBCD0,0xfa		;initialize digits 1 and 0
binbcd_101:	
	subi	tBCD0,-0x10		;
    subi	fbin0,10		;subi fbin,10^1
    brcc	binbcd_101		;
    add	tBCD0,fbin0		;LSD
	ret

;***************************************************************************
;*
;* Add32 == 32+32 Bit Unsigned Addition
;*
;* add1L::add1H  +  add2L::add2H  =  add1L::add1H
;*     item		item		 sum
;* r20r21r22r23  +  r16r17r18r19  =  r20r21r22r23
;*
;***************************************************************************
//r16	; item 2 byte 0 (LSB)
//r17	; item 2 byte 1
//r18	; item 2 byte 2
//r19	; item 2 byte 3 (MSB)
//r20	; item 1 byte 0 (LSB)
//r21	; item 1 byte 1
//r22	; item 1 byte 2
//r23	; item 1 byte 3 (MSB)
                                    
Add24sign:	
		brts	Sub24sign	;
Add24:	
		add	r20,r16	;Add low bytes
        adc	r21,r17	;Add higher bytes with carry
        adc	r22,r18	;
        adc	r23,r19	;
        ret

;***************************************************************************
;*
;* Sub32 == 24-24 Bit Unsigned Subtraction
;*
;* sub1L::sub1H  -  sub2L::sub2H  =  sub1L::sub1H
;*   minuend	     subtrahend       difference
;* r20r21r22  -  r16r17r18  =  r20r21r22
;*
;***************************************************************************
//r16	; subtrahend byte 0 (LSB)
//r17	; subtrahend byte 1
//r18	; subtrahend byte 2 (MSB)
//r20	; minuend byte 0 (LSB)
//r21	; minuend byte 1
//r22	; minuend byte 2 (MSB)
                                    
Sub24sign:	
		clt			;sign +
Sub24:	sub	r20,r17	;Subtract low bytes
        sbc	r21,r17	;Subtract higher bytes with carry
        sbc	r22,r18	;
        brcc	Return24u	;return clear carry if result>=0
        set		;sign -
Neg24:	subi	r20,1 	;if result<0
        sbci	r21,0 	;   neg result
        sbci	r22,0 	;   (dec result)
Com24:	com	r20		;	&
        com	r21		;   (com result)
        com	r22		;   return set carry after com
Return24u:	
		ret

;***************************************************************************
;*
;* "BCD_INC" - 2-digit packed BCD incrementation
;*
;* This subroutine increments unsigned 2-digit BCD number "BCD1" 
;* The result is returned in "BCD1", and the overflow carry in r17.
;***************************************************************************
BCD_ADD:
	push r18
    ldi	r18, 6	;value to be added later
	//ldi r17, 0x01
    add	r16,r17	;add the numbers binary
    clr	r17		;clear BCD carry
    brcc	BCD_INC_0		;if carry not clear
    ldi	r17,1		;    set BCD carry
BCD_INC_0:	
	brhs	BCD_INC_1		;if half carry not set
    add	r16,r18	;    add 6 to LSD
    brhs	BCD_INC_2		;    if half carry not set (LSD <= 9)
    subi	r16,6		;        restore value
    rjmp	BCD_INC_2		;else
BCD_INC_1:	
	add	r16,r18	;    add 6 to LSD
BCD_INC_2:	
	swap	r18
    add	r16,r18	;add 6 to MSD
    brcs	BCD_INC_4		;if carry not set (MSD <= 9)
    sbrs	r17,0		;    if previous carry not set
    subi	r16,$60	;	restore value 
BCD_INC_3:
	pop r18	
	ret			;else
BCD_INC_4:	
	ldi	R17,1		;    set BCD carry

    pop r18
	ret

;***************************************************************************
;*
;* "BCD_DEC" - 2-digit packed BCD decrementation
;*
;* This subroutine decrements the unsigned 2-digit BCD number 
;* "R16" and "R17=1" (R16 - 1). The result is returned in "R16", and 
;* the underflow carry in "R17".
;*
;***************************************************************************
BCD_DEC:
    subi	R16,1		;subtract the numbers binary
    clr	R17
    brcc	BCD_DEC_0	;if carry not clear
    ldi	R17,1			;store carry in BCDB1, bit 0
BCD_DEC_0:	
	brhc	BCD_DEC_1	;if half carry not clear
    subi	R16,$06		;LSD = LSD - 6
BCD_DEC_1:	
	sbrs	R17,0		;if previous carry not set
    ret					;return
    subi	R16,$60		;subtract 6 from MSD
    ldi	R17,1			;set underflow carry
    brcc	BCD_DEC_2	;if carry not clear
    ldi	R17,1			;clear underflow carry	
BCD_DEC_2:	
ret			
                                    